#ifndef LOCALIZATION_VISION_MODEL_H_
#define LOCALIZATION_VISION_MODEL_H_

#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <libplayerc++/playerc++.h>
#include <libplayerinterface/player.h>
#include <core/macros.h>
#include <core/synced_lamp_centroids.h>

class VisionModel {
public:
  // Empirical uncertainty constants
  static const double kVarR = 0.25;
  static const double kVarTheta = 0.10;

  VisionModel(const std::vector<player_point_2d> &mapped_lamps,
          SyncedLampCentroids *lamp_centroids);
  ~VisionModel();

  void UpdateBelief(player_pose2d *mean, gsl_matrix *covariance);
private:
  static const bool kDebugMode = false;

  // Matrices used in the calculations
  gsl_matrix * z_predicted_;
  gsl_matrix * z_observed_;
  gsl_matrix * H_;
  gsl_matrix * S_;
  gsl_matrix * S_inverse_;
  gsl_matrix * Q_;
  gsl_matrix * K_;
  gsl_matrix * I_;
  gsl_matrix * aux_3x1_;
  gsl_matrix * aux_3x2_;
  gsl_matrix * aux_3x3_;
  gsl_permutation * permutation_;

  // Coordinates of the mapped lamps relative to the global map's frame of reference
  std::vector<player_point_2d> mapped_lamps_;

  // Synchronized list of the lamps detected by the robot's camera
  SyncedLampCentroids *lamp_centroids_;

  DISALLOW_IMPLICIT_CONSTRUCTORS(VisionModel);
};

#endif
